#include <stdio.h>

#include "lapackUtil.h"
#include "comp_llpmatrix.h"
#include "lpmatrix.h"
#include "pcdwrapper.h"

#include <string>
#include <fstream>
#include <iostream>

#include <boost/algorithm/string.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/foreach.hpp>




//#include "arlnsmat.h"
//#include "arlsnsym.h"

double* readConnectivityFile(char* filename, int& n) {
    if (NULL == filename) {
        printf("error in readConnectivityFile: null filename\n");
        return NULL;
    }

    std::ifstream ifs;
    std::string line;

    std::vector<std::string> stvec;

    double* result = NULL;

    ifs.open(filename);

    std::getline(ifs, line);

    try {
        n = boost::lexical_cast<int>(line);
    } catch (boost::bad_lexical_cast&) {
        printf("readConnectivityFile: lexical_cast fail at n \n");
        return result;
    }

    printf("# of vertices: %d\n", n);
    result = new double[n * n];
    if (NULL == result) {
        printf("readConnectivityFile: allocation fail\n");
        return NULL;
    }

    for (int i = 0; i < n * n; ++i) {
        result[i] = 0;
    }

    int x, y, piv;

    if (ifs.is_open()) {
        do {

            std::getline(ifs, line);
            if (line.size() <= 0) continue;
            boost::algorithm::split(stvec, line, boost::algorithm::is_any_of(" \n"));

            try {
                x = boost::lexical_cast<double>(stvec[0]);
                y = boost::lexical_cast<double>(stvec[1]);

                piv = x * n + y; //use col-major matrix for lapack processing
                result[piv] = -1;
                piv = y * n + x; //use col-major matrix for lapack processing
                result[piv] = -1;
            } catch (boost::bad_lexical_cast&) {
                printf("readConnectivityFile: lexical_cast fail\n");
            }



        } while (!ifs.eof());

        double d;
        for (int i = 0; i < n; ++i) {
            d = 0;
            piv = i*n;
            for (int j = 0; j < n; ++j) {
                d += result[piv + j];
            }
            piv = i * n + i;
            result[piv] = -d;
        }

    } else {
        printf("readConnectivityFile: open file failed\n");
        delete[] result;
        return NULL;
    }

    return result;
}

std::vector<std::map<int, double> > readConnectivityFileSparse(char* filename, int& n) {
    std::vector<std::map<int, double> > result;

    if (NULL == filename) {
        printf("error in readConnectivityFileSparse: null filename\n");
        return result;
    }

    std::ifstream ifs;
    std::string line, s;

    std::vector<std::string> stvec;

    int i, row, col, val;


    ifs.open(filename);

    std::getline(ifs, line);

    try {
        n = boost::lexical_cast<int>(line);
    } catch (boost::bad_lexical_cast&) {
        printf("readConnectivityFileSparse: lexical_cast fail at n \n");
        return result;
    }

    result.resize(n);
    printf("# of vertices: %d\n", n);

    int pos[2];
    std::pair<int, double> colelement;

    if (ifs.is_open()) {
        do {

            std::getline(ifs, line);
            if (line.size() <= 0) continue;
            boost::algorithm::split(stvec, line, boost::algorithm::is_any_of(" \n"));

            try {
                /*
                x = boost::lexical_cast<double>(stvec[0]);
                y = boost::lexical_cast<double>(stvec[1]);
                 */
                i = 0;

                BOOST_FOREACH(s, stvec) {
                    if (s.length() > 0) {
                        val = boost::lexical_cast<int>(s);
                        if (i < 2) {
                            pos[i] = val;
                        }
                        ++i;
                    }
                }

                row = pos[0];
                col = pos[1];

                if (row == col) {
                    cerr << "found extra diagonal connection " << row << " \n";
                }

                //if(row==959 || col==959) cerr<<"959 found\n";

                colelement.first = col;
                colelement.second = 1;
                result[row].insert(colelement);
                colelement.first = row;
                colelement.second = 1;
                result[col].insert(colelement);

            } catch (boost::bad_lexical_cast&) {
                printf("readConnectivityFileSparse: lexical_cast fail\n");
            }

        } while (!ifs.eof());



        double sum;
        for (int i = 0; i < n; ++i) {
            sum = 0;

            BOOST_FOREACH(colelement, result[i]) {
                if (colelement.first == i) {
                    cerr << "found extra diagonal element " << i << " " << colelement.second << "\n";
                } else {
                    sum += colelement.second;
                }

            }

            colelement.first = i;
            colelement.second = -sum;
            result[i].insert(colelement);

            if (sum == 0) {
                cerr << "found isolated point " << i << "\n";
            }
        }

    } else {
        printf("readConnectivityFileSparse: open file failed\n");
    }


    return result;
}

int main(int argc, char* argv[]) {
    if (argc < 3) {
        printf("not enough parameters\n");
        return -1;
    }

    printf("running...\n");


    int n;
    //array = readConnectivityFile(argv[1], n);
    /*
        if (NULL == array) {
            printf("Memory Allocation failed...\n");
            return -1;
        }
     */
    /*
        lambda = new double[n];

        solve_eigen(array, lambda, n);

        writeDoubleRowSqMatrix(array, n, argv[2]);
        writeDoubleRow(lambda, n, argv[3]);

        delete[] lambda;
     */

    std::vector<std::map<int, double> > array;

    array = readConnectivityFileSparse(argv[1], n);

    std::ofstream ofs;
    char buf[300];
    int piv;
    double val;
    std::string s;

    ofs.open(argv[2]);


    std::pair<int, double> element;
    for (int i = 0; i < n; ++i) {
        //although this is col-major matrix, it is symmetric

        BOOST_FOREACH(element, array[i]) {

            sprintf(buf, "%d %d %f\n", i + 1, element.first + 1, element.second);
            s.clear();
            s.append(buf);

            ofs << s;
        }
    }

    /*
        sprintf(buf, "%4.20f ", val);
        s.clear();
        s.append(buf);

        ofs << s;
     */
    ofs.close();


    return 0;
}
